function [p] = lla2ecef_cg( latLonAlt, f, RE ) %#codegen
%LLA2ECEF_CG 将经纬高转换成地球系坐标，适用于代码生成版本
%
% Tips:
% # 地球坐标系选择GwEN
%
% Input Arguments:
% # latLonAlt: m*3矩阵，每行3个元素分别对应纬度、经度、高度，单位分别为rad、rad、m
% # f: 标量，地球模型椭球体的扁率，无单位
% # RE: 标量，地球模型椭球体的半长轴，单位m
%
% Output Arguments:
% # p: m*3矩阵，每行3个元素分别对应地球系坐标x、y、z，单位为m
%
% References:
% #《应用导航算法工程基础》“地球系坐标与经纬高的转换”

m = size(latLonAlt, 1);
p = coder.nullcopy(NaN(m, 3));
eSq = 1 - (1-f)^2; 
RN = RE ./ sqrt(1 - eSq * sin(latLonAlt(:, 1)).^2);
p(:, 1) = (RN+latLonAlt(:, 3)) .* cos(latLonAlt(:, 1)) .* cos(latLonAlt(:, 2));
p(:, 2) = (RN+latLonAlt(:, 3)) .* cos(latLonAlt(:, 1)) .* sin(latLonAlt(:, 2));
p(:, 3) = (RN*(1-eSq)+latLonAlt(:, 3)) .* sin(latLonAlt(:, 1));
end